#!/usr/bin/env python3
"""
导航状态监控与反馈节点

该节点是一个用于提升用户体验的辅助工具。其主要功能是：
1.  **监控导航系统日志**: 它通过订阅ROS的中央日志话题 `/rosout_agg` 来监听
    系统中所有节点产生的日志消息。
2.  **识别恢复行为**: 它专门筛选出来自 'move_base' 导航核心节点的日志，
    并检测其中是否包含表明机器人遇到困难并正在尝试自我恢复的关键词
    （如 "Clearing costmap", "Rotate recovery"）。
3.  **主动语音反馈**: 一旦检测到机器人正在尝试恢复，它会立即通过TTS
    （文本转语音）发布一条友好的提示（例如，"检测到障碍物，正在尝试绕行"），
    主动告知用户当前情况，避免用户在机器人卡住时感到困惑。
4.  **频率限制**: 内置了冷却机制，防止在短时间内因密集的日志而重复播报，
    骚扰用户。
"""
import rospy
from rosgraph_msgs.msg import Log
from std_msgs.msg import String
import time

class NavigationMonitor:
    """
    封装了导航日志监控和语音反馈逻辑的类。
    """
    def __init__(self):
        """
        初始化NavigationMonitor节点。
        """
        rospy.init_node('navigation_monitor', anonymous=True)
        
        # --- ROS Publishers & Subscribers ---
        # 发布器，用于在检测到特定情况时发送语音播报指令
        self.tts_pub = rospy.Publisher('tts_text', String, queue_size=10)
        
        # 订阅器，监听聚合的ROS日志话题
        rospy.Subscriber('/rosout_agg', Log, self.log_callback)
        
        # --- 状态与频率控制 ---
        # 记录上一次播报的时间，用于实现频率限制
        self.last_speech_time = 0
        # 播报的冷却时间（秒），避免短时间内重复播报
        self.speech_cooldown = 20
        
        # 用于检测move_base恢复行为的关键词列表
        self.recovery_keywords = [
            "Starting recovery behavior",
            "Rotate recovery",
            "Clearing costmap"
        ]
        
        rospy.loginfo("导航监控节点已启动，正在监听move_base日志...")

    def log_callback(self, msg: Log):
        """
        处理来自 '/rosout_agg' 话题的每一条日志消息。
        """
        # 1. 只关心来自 move_base 节点的日志消息
        if msg.name == '/move_base':
            # 2. 检查是否已超过冷却时间，避免消息轰炸
            current_time = time.time()
            if current_time - self.last_speech_time > self.speech_cooldown:
                # 3. 遍历关键词列表，检查日志内容是否包含任一关键词
                for keyword in self.recovery_keywords:
                    if keyword in msg.msg:
                        rospy.loginfo(f"检测到move_base恢复行为: '{msg.msg}'")
                        
                        # 4. 发布用户友好的语音提示
                        feedback_message = "检测到障碍物，正在尝试绕行，请稍候。"
                        self.tts_pub.publish(feedback_message)
                        rospy.loginfo(f"已发布TTS反馈: '{feedback_message}'")
                        
                        # 5. 更新上一次播报的时间戳
                        self.last_speech_time = current_time
                        
                        # 6. 找到一个匹配项后即可退出循环，避免对同一条日志消息重复播报
                        break

if __name__ == '__main__':
    try:
        monitor = NavigationMonitor()
        rospy.spin() # 保持节点运行，等待回调
    except rospy.ROSInterruptException:
        rospy.loginfo("导航监控节点关闭。")
        pass 